#include <avr/io.h>
#include <util/delay.h>	//for microsecond delays
#include <stdio.h>
#include <string.h>

#include "mouse_sensor.h"
#include "uart.h"
#include "adns3080_srom.h"

/***********************************************************
* void initializeMouseSensors(void)
* 		Sets up the mouse sensor variables
*		 and hardware configs
************************************************************/
void initializeMouseSensors(void)
{
	//Set initial variable values
	for(int i=0; i<2; i++)
	{
		sum_x[i] = 0;
		sum_y[i] = 0;
		acc_x[i] = 0;
		acc_y[i] = 0;
	}
	xval = xval2 = yval = omega = 0;

	SPI_MasterInit();
	Init_Opt();
}

/***********************************************************
* void SPI_MasterInit(void)
* 		Sets up the SPI with the MCU as master
************************************************************/
void SPI_MasterInit(void)
{
	// Setup Port B outputs
	DDRB = 0b10111111; //MISO is the only input
	PORTB |= (1<<CS2)|(1<<CS1);
	// Enable SPI, Master, set clock rate fck/16 
	SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);  //SPCR = SPI Control Register
	// Use MSB FIRST: DORD = 0
	
	// ADNS-3080 reads MOSI on falling edge of SCLK
	// Outputs MISO on rising edge SCLK
	// Shift SCK-edge: rising, capture SCK-edge: falling	
	SPCR |= (1<<CPOL) | (1<<CPHA);	// CPOL = 1, CPHA = 1
}

/***********************************************************
* void Init_Opt(void)
* 		Initialize the optical sensors
* 		Resets the sensors to intialize them
* 		Downloads the SROM and sets config regs
************************************************************/
void Init_Opt(void)
{		
	// Pulse reset high for both sensors - must be 10 us
	PORTB |= (1<<RESET1)|(1<<RESET2);
	_delay_us(10);
	PORTB &= ~((1<<RESET1)|(1<<RESET2));

	//Write the SROM for sensor 1
	srom_result = SROM(1);	
	if (srom_result) {
		sprintf(temp_str,"SROM 1 Valid: %d\n\r", srom_result);
		uart_send_str(temp_str,strlen(temp_str));
	}
	else {
		sprintf(temp_str,"SROM 1 NOT Valid: %d\n\r", srom_result);
		uart_send_str(temp_str,strlen(temp_str));
	}
	
	//Write the SROM for sensor 2
	srom_result = SROM(2);
	if (srom_result) {
		sprintf(temp_str,"SROM 2 Valid: %d\n\r", srom_result);
		uart_send_str(temp_str,strlen(temp_str));
	}
	else {
		sprintf(temp_str,"SROM 2 NOT Valid: %d\n\r", srom_result);
		uart_send_str(temp_str,strlen(temp_str));
	}
	
	//Set the configuration registers
	//SPI_MasterTransmit(1,0x8a,0b01000000);  //BIT6=Shutter mode on
	//SPI_MasterTransmit(2,0x8a,0b01000000);  //BIT6=Shutter mode on

	//Disable serial port pull-ups
	SPI_MasterTransmit(1,0x8b,0b00000100);  
	SPI_MasterTransmit(2,0x8b,0b00000100);
}

/***********************************************************
* void SROM(sensor)
* 		Downloads the SROM data to the specified sensor
* 		Calls Burst_Mode to send the data
************************************************************/
char SROM(unsigned char sensor)
{
	char valid;
	//Toggle reset
	if (sensor==1) {
		PORTB |= (1<<RESET1);
		_delay_us(10);
		PORTB &= ~(1<<RESET1);
	} else {
		PORTB |= (1<<RESET2);
		_delay_us(10);
		PORTB &= ~(1<<RESET2);
	}

	//Write 0x44 to reg 0x20
	SPI_MasterTransmit(sensor,(0x80|0x20),0x44);
	_delay_us(150);
	//Write 0x07 to reg 0x23
	SPI_MasterTransmit(sensor,(0x80|0x23),0x07);
	_delay_us(150);
	//Write 0x88 to register 0x24
	SPI_MasterTransmit(sensor,(0x80|0x24),0x88);
	//Wait >= 1 frame period
	_delay_us(500);
	//Write 0x18 to register 0x14 (SROM_Enable register)
	SPI_MasterTransmit(sensor,(0x80|0x14),0x18);
	_delay_us(150);
	
	//Begin burst mode write of data file to register 0x60 (SROM_Load register)
	Burst_Mode(sensor,(0x80|0x60), AVAGO_SROM);

	_delay_us(150);
	//Read the SROM_ID reg to verify successful SROM download
	valid = SPI_MasterTransmit(sensor, 0x1f,0xaa);
	
	//Returns the contents of the SROM_ID reg
	return valid;
}

/***********************************************************
* void Burst Mode(sensor, addr, data[])
* 		Send data to the ADNS-3080 using Burst Mode
*		This function is specifically set up to send the 
*		1986 bytes for the SROM download.
************************************************************/
void Burst_Mode(unsigned char sensor, char addr, char data[])
{
	char junk;
	// Null Pointer Check
	if(!data){ return;}
	// Set CS to low to enable transmission
	if (sensor==1) {
		PORTB &= ~(1<<CS1);
	} else {
		PORTB &= ~(1<<CS2);
	}
	// Start transmission
	SPDR = addr;
	while(!(SPSR & (1<<SPIF))); //need 10 us between data bytes
	_delay_us(50);
	for(int index=0; index<1986; index++)
	{
		SPDR = data[index];
		// Wait for transmission complete
		while(!(SPSR & (1<<SPIF))); //need 10 us between data bytes
		junk = SPDR;  //receive junk that comes back
		_delay_us(150);
	}
	
	// Set CS to high at end of transmission
	if (sensor==1) {
		PORTB |= (1<<CS1);
	} else {
		PORTB |= (1<<CS2);
	}
	_delay_us(4);	//Hold CS high for 4 us to exit burst mode
}

/***********************************************************
* char SPI_MasterTransmit(sensor,byte1,byte2)
* 		Transmits 2 bytes to the specified ADNS-3080
* 		Returns the second received byte
*		Can be used to read or write regs
*		READ: MSB=0, WRITE: MSB=1
************************************************************/
char SPI_MasterTransmit(unsigned char sensor, unsigned char byte1, unsigned char byte2)
{
	signed char input,junk; 
	// Set CS to low to enable transmission
	if (sensor==1) {
		PORTB &= ~(1<<CS1);
	} else {
		PORTB &= ~(1<<CS2);
	}
	// Start transmission
	SPDR = byte1;  //SPDR = SPI Data Register
	// Wait for transmission complete
	while(!(SPSR & (1<<SPIF)));  //SPSR = SPI Status Register, SPIF = tx complete
	//_delay_us(10);
	junk = SPDR;  //receive junk that comes back
	_delay_us(75);
	SPDR = byte2;
	while(!(SPSR & (1<<SPIF)));
	//_delay_us(10);
	input = SPDR; 
	// Set CS to high at end of transmission
	if (sensor==1) {
		PORTB |= (1<<CS1);
	} else {
		PORTB |= (1<<CS2);
	}
	//Return the MOSI data from the second transmission
	return input;
}

/***********************************************************
* void Calc_Pos(void)
* 		Calculates the position based off the sensor readings
************************************************************/
void Calc_Pos(void)
{
	//Calculate x,y,omega

	xval = deltaX_X1_coef*acc_x[0] + deltaX_X2_coef*acc_x[1] + deltaX_Y1_minus_Y2_coef*(acc_y[0]-acc_y[1]);
	xval2 += xval;	//this cured a problem with doing xval += ...
	yval += deltaY_X1_coef*acc_x[0] + deltaY_X2_coef*acc_x[1] + deltaY_Y1_coef*acc_y[0] + deltaY_Y2_coef*acc_y[1];
	omega += omega_X1_minus_X2_coef*(acc_x[0]-acc_x[1]) + omega_Y1_minus_Y2_coef*(acc_y[0]-acc_y[1]);
		
	//Reset the accumulated x and y values for both sensors
	acc_x[0] = 0;
	acc_x[1] = 0;	
	acc_y[0] = 0;
	acc_y[1] = 0;	
}

/***********************************************************
* void Read_Data(sensor)
* 		Reads the motion data from the specified sensor
************************************************************/
char Read_Data(unsigned char sensor) 
{
	//Read motion reg
	data = SPI_MasterTransmit(sensor,0x02,0x00);
	data_ready = data&0x80;	//set flag for motion since last reg read
	//If it moved, read delta_x and delta_y regs
	if(data_ready) {
		//PORTD &= ~(1<<PD2);
		_delay_us(75);
		//Read delta_x register	
		x[sensor] = SPI_MasterTransmit(sensor,0x03,0x00);
		

		_delay_us(50); //required delay between reads
		//Read delta_y register
		y[sensor] = SPI_MasterTransmit(sensor,0x04,0x00);
		
		//Add the new delta_x to the current count
		sum_x[sensor] += x[sensor];
		acc_x[sensor] += x[sensor];

		//Add the new delta_y to the current count
		sum_x[sensor] += y[sensor];
		acc_y[sensor] += y[sensor];
	}	
	else
	{
		//PORTD |= (1<<PD2);
		//Set the x and y motion values to 0
		x[sensor] = 0;
		y[sensor] = 0;
	}

	//Return overflow bit
	return data&0x10;	
}

/***********************************************************
* void ReadSurfaceQuality(void)
* 		Reads the surface quality from the mouse sensors
************************************************************/
void ReadSurfaceQuality(void)
{
	squal[0] = SPI_MasterTransmit(0,0x05,0x00);
	squal[1] = SPI_MasterTransmit(1,0x05,0x00);
}
